Feature: New POLYLINE command in Pilz planner for space trajectory generation #3610

AimanHaidar · 2025-11-10T19:04:59Z

Description

This pull request adds a new Pilz Industrial Planner command: POLYLINE, which enables planning Cartesian polyline-space trajectories using waypoints.

Added Classes

The workflow of these classes follows the same structure as the existing CIRC and LIN commands.

TrajectoryGeneratorPolyline
Main class responsible for receiving the MotionPlanRequest and setting up the polyline-space path.
PathPolylineGenerator
Handles the generation of polyline paths using KDL::Path_RoundedComposite.
This class provides:
- polylineFromWaypoints() – Adds and validates waypoints.
- computeBlendRadius() – Computes the maximum feasible rounding radius, scaled by the smoothness factor.
- checkConsecutiveColinearWaypoints() – Detects and throws an error if three consecutive colinear points are found (handled in setPolylinePath()).

Notes:

This feature requires the smoothness_level field in MotionPlanRequest (introduced in PR #190).

it include what was pulled at Feature/make use of max cartesian speed in MotionPlanRequest with pilz planner #3608

Limitations

The planner cannot generate trajectories that contain three or more consecutive colinear waypoints.
This is a limitation of KDL::Path_RoundedComposite.
When such a case is detected, an error is thrown and the plan is rejected.

Usage Example

An example of how to use the POLYLINE command with MotionSequenceItem:

moveit_msgs::msg::MotionSequenceItem sequence_item;
sequence_item.req.planner_id = "POLYLINE"
std::vector<geometry_msgs::msg::Pose> waypoints = // ...

geometry_msgs::msg::PoseStamped msg;
msg.header.frame_id = reference_frame;

for (const auto& waypoint : waypoints)
{
  msg.pose = waypoint;
  moveit_msgs::msg::PositionConstraint pos_constraint;
  pos_constraint.header.frame_id = reference_frame;
  pos_constraint.link_name = commanded_link;
  pos_constraint.constraint_region.primitive_poses.resize(1);
  pos_constraint.constraint_region.primitive_poses[0] = waypoint;
  pos_constraint.weight = 1.0;
  sequence_item.req.path_constraints.position_constraints.push_back(pos_constraint);
}

// Add the final point as the goal constraint
msg.pose = waypoints.back();
sequence_item.req.goal_constraints.push_back(
    kinematic_constraints::constructGoalConstraints(link_name, msg));

if (zero_blend_at_end)
  sequence_item.blend_radius = 0.0;  // No blending for the last waypoint

// Send the Sequence Item as in the MoveIt tutorials

Tests

triangles with different smoothness:

points generated from external parametric curve:

Checklist

Required by CI: Code is auto formatted using clang-format
Extend the tutorials / documentation reference
Document API changes relevant to the user in the MIGRATION.md notes
Create tests, which fail without this PR reference
Include a screenshot if changing a GUI
While waiting for someone to review your request, please help review another open pull request to support the maintainers

due to derivatives discontinuouties normal composite fail to Pose IK

…-generator

AimanHaidar · 2025-11-19T19:36:46Z

@v4hn
@EzraBrooks
@nbbrooks

May you review this ?

EzraBrooks

Just one comment otherwise LGTM.

moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp

AndyZe

I'm not sure "free" is the right title. Suggesting some other names:

variable blend --> variableBlendFromWaypoints()
motion sequence --> sequenceFromWaypoints(). This is my favorite.

A free space planner typically means something like A* that is free to take any route to the goal.

AndyZe

Your triangle example seems to violate this? Did something change:

The planner cannot generate trajectories that contain three or more consecutive colinear waypoints (e.g., straight lines)

AimanHaidar · 2025-11-23T06:39:52Z

Your triangle example seems to violate this? Did something change:

The planner cannot generate trajectories that contain three or more consecutive colinear waypoints (e.g., straight lines)

the triangle example uses only triangle's vertices. what I meant there by "straight lines" is pure straight line.
if you give three points on the same line or give two points they make collinearity with the start point.
the restrictions come from
lines 96 to 102

AimanHaidar · 2025-11-23T07:15:16Z

I'm not sure "free" is the right title. Suggesting some other names:

variable blend --> variableBlendFromWaypoints()

motion sequence --> sequenceFromWaypoints(). This is my favorite.

A free space planner typically means something like A* that is free to take any route to the goal.

Right, FREE is not appropriate.
naming is a hard part!

sea-bass · 2025-11-24T02:29:29Z

I'm not active on here these days, but I saw this pop up on my feed. I wanted to point out that this feature seems to do the same thing as https://moveit.picknik.ai/main/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.html#sequence-of-multiple-segments -- so I would recommend 2 things for the author and reviewers:

Consider whether this new feature is either redundant with the above, or whether it's a better option that could allow removing the above. For example, if the existing blending feature actually allows blending different motion types (PTP, LIN, and CIRC), this PR may not be a full replacement. But I would check if this is the case.
If the decision is to keep this around, instead of making this be a new motion type, I'd keep it as LIN motion, but now if you specify intermediate waypoints as part of the message it automatically does the blending. After all, this is a blend of LIN motion segments.

Hope this helps more so than adds noise!

AimanHaidar · 2025-11-24T05:37:51Z

I'm not active on here these days, but I saw this pop up on my feed. I wanted to point out that this feature seems to do the same thing as https://moveit.picknik.ai/main/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.html#sequence-of-multiple-segments -- so I would recommend 2 things for the author and reviewers:
1. Consider whether this new feature is either redundant with the above, or whether it's a better option that could allow removing the above. For example, if the existing blending feature actually allows blending different motion types (PTP, LIN, and CIRC), this PR may not be a full replacement. But I would check if this is the case.

2. If the decision is to keep this around, instead of making this be a new motion type, I'd keep it as `LIN` motion, but now if you specify intermediate waypoints as part of the message it automatically does the blending. After all, this is a blend of LIN motion segments.
Hope this helps more so than adds noise!

@sea-bass
Thanks, This is what I have tried before. I used LIN command to generate general curves so I divided the curve into small segments but LIN seems to sample these small segments into many points as long lines, which ends up producing a lot of unnecessary points.
I noticed this when I visualized the path in RViz: using the LIN and Sequence method produced very dense points.

I tested both methods using points sampled from the ellipse:

$$ x = 0.1 \cos t,\qquad y = 0.05 \sin t,\qquad z = 0.0 $$

with $t \in [0,\ 2.0],\quad dt = 0.01$

the number of points in the planned joint_trajectory with the same max_cartesian_speed. I got:

# This is with LIN commands and Sequence
[INFO] [1763959333.823367295] [cartesian_planning]: TrajPoints 408
# This is with the new Command 
[INFO] [1763960677.326358520] [commands_loader]: TrajPoints : 29

But Why 200 ellipse waypoints to 29 joint trajectory points???

note: the number of joint trajectory points increase with decrease max_cartesian_speed

AimanHaidar · 2025-12-01T14:28:20Z

@sea-bass
@MatthijsBurgh
@AndyZe
@EzraBrooks

I have changed the name to POLYLINE
I think PATH is more general, but POLYLINE is the best one describes creating path of connecting many points with lines but the difference is there is arcs in between to smooth the path or make its derivative continuous.

If possible, I would like to rename it to SPOLYLINE, where the ‘S’ indicates smooth.

moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_polyline.cpp

codecov · 2025-12-01T17:44:58Z

Codecov Report

❌ Patch coverage is 29.59184% with 138 lines in your changes missing coverage. Please review.
✅ Project coverage is 46.12%. Comparing base (760277f) to head (7ff2294).
⚠️ Report is 1 commits behind head on main.

Files with missing lines	Patch %	Lines
...tion_planner/src/trajectory_generator_polyline.cpp	5.27%	72 Missing ⚠️
...ial_motion_planner/src/path_polyline_generator.cpp	0.00%	49 Missing ⚠️
...n_planner/src/planning_context_loader_polyline.cpp	53.34%	7 Missing ⚠️
...ustrial_motion_planner/path_polyline_generator.hpp	0.00%	6 Missing ⚠️
...l_motion_planner/trajectory_generator_polyline.hpp	20.00%	4 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3610      +/-   ##
==========================================
- Coverage   46.19%   46.12%   -0.06%     
==========================================
  Files         720      726       +6     
  Lines       59181    59370     +189     
  Branches     7594     7614      +20     
==========================================
+ Hits        27333    27381      +48     
- Misses      31681    31822     +141     
  Partials      167      167

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

EzraBrooks · 2025-12-01T19:13:19Z

Looks like you're failing CI - it would also be nice to get some code coverage here. a 0% covered diff makes me anxious!

AimanHaidar · 2025-12-01T20:11:23Z

Looks like you're failing CI - it would also be nice to get some code coverage here. a 0% covered diff makes me anxious!

The failure is due to smoothness_level . see the first note in the PR description

about code coverage (codecov) I really don't understand what it is!!
I will learn what this then reply.

edit algorithms loader to include the 4th POLYLINE COMMAND

AimanHaidar · 2025-12-02T07:36:14Z

Mr @EzraBrooks I have added two tests for max_cartesian_speed in 0120591

also, remove unrelated fix 👇 introduced in #3604

…loat" This reverts commit c86e3b2.

AimanHaidar · 2025-12-03T17:16:50Z

after I updated the branch, the CI / hamble failed due to something not related to pilz ?!

AimanHaidar · 2025-12-03T20:55:12Z

@EzraBrooks
Now, all required CI checks have passed

EzraBrooks · 2025-12-03T21:30:21Z

what about @MatthijsBurgh's comment regarding .h files?

AimanHaidar · 2025-12-04T17:12:00Z

@EzraBrooks
It is the time to open PR for moveit2_tutorial.

AimanHaidar added 18 commits October 24, 2025 22:12

Format max_velocity and max_acceleration for YAML output as float

c86e3b2

make use of max_cartesian_speed in MotionSequenceRequest

38aac46

use feature in circ command

7d70a27

organize into function

0bb71f2

Merge branch 'fix/joint-limits-crash' into dev

b28883e

add free path planner using composite kdl path

ce9099d

fix include files and add it to plugin

a0bc8e4

convert to roundedComposite

4e62e81

due to derivatives discontinuouties normal composite fail to Pose IK

fix bug andadd check of near start point to curve tip

e9f63ae

fix bug in set max cart speed before set of path

4008bdc

Merge branch 'feature/use-max-cartesian-speed' into feature/free-path…

b8e8de3

…-generator

add check request validation

99f146b

add function to compute the max rounding circle radius

24d27ce

add close points check to avoid Not_Feasable error

84a8e94

add helper class for generate free paths

749d7e4

clean code and add copyright!

440a77d

Merge branch 'moveit:main' into feature/free-path-generator

c6a0389

handle colinear points errors

7e08974

github-project-automation bot added this to MoveIt Nov 10, 2025

This was referenced Nov 10, 2025

add smoothness_level for new polyline path pilz planner command moveit/moveit_msgs#190

Merged

Feature/make use of max cartesian speed in MotionPlanRequest with pilz planner #3608

Closed

correct error massages and fix bug

5072fbd

EzraBrooks reviewed Nov 22, 2025

View reviewed changes

moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp Outdated Show resolved Hide resolved

AndyZe reviewed Nov 22, 2025

View reviewed changes

AimanHaidar added 2 commits December 1, 2025 17:01

change name to polyline

5f7a8ba

change files' names

d5ce36c

AimanHaidar closed this Dec 1, 2025

AimanHaidar deleted the feature/free-path-generator branch December 1, 2025 14:11

github-project-automation bot moved this to ✅ Done in MoveIt Dec 1, 2025

AimanHaidar restored the feature/free-path-generator branch December 1, 2025 14:15

AimanHaidar reopened this Dec 1, 2025

EzraBrooks reviewed Dec 1, 2025

View reviewed changes

moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_polyline.cpp Show resolved Hide resolved

AimanHaidar added 2 commits December 1, 2025 19:48

rename

b1df424

fix jazzy-ci issue

61c8742

AimanHaidar changed the title ~~Feature: New FREE command in Pilz planner for free-space trajectory generation~~ Feature: New POLYLINE command in Pilz planner for space trajectory generation Dec 1, 2025

add test for the cartesian max speed

0120591

edit algorithms loader to include the 4th POLYLINE COMMAND

AimanHaidar added 3 commits December 2, 2025 10:37

Revert "Format max_velocity and max_acceleration for YAML output as f…

ef3294f

…loat" This reverts commit c86e3b2.

Merge branch 'main' into feature/free-path-generator

7bd69b7

fix clang-tidy failures for rolling

d60d8dd

EzraBrooks approved these changes Dec 3, 2025

View reviewed changes

delete .h files

7ff2294

EzraBrooks enabled auto-merge (squash) December 4, 2025 14:06

EzraBrooks approved these changes Dec 4, 2025

View reviewed changes

EzraBrooks merged commit 2002fb7 into moveit:main Dec 4, 2025
9 of 10 checks passed

AimanHaidar mentioned this pull request Dec 5, 2025

Update pilz tutorials for POLYLINE planner moveit/moveit2_tutorials#1075

Open

2 tasks

Feature: New POLYLINE command in Pilz planner for space trajectory generation #3610

Feature: New POLYLINE command in Pilz planner for space trajectory generation #3610

Uh oh!

Conversation

AimanHaidar commented Nov 10, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Added Classes

Limitations

Usage Example

Tests

triangles with different smoothness:

points generated from external parametric curve:

Checklist

Uh oh!

AimanHaidar commented Nov 19, 2025

Uh oh!

EzraBrooks left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

AndyZe left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

AndyZe left a comment

Choose a reason for hiding this comment

Uh oh!

AimanHaidar commented Nov 23, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

AimanHaidar commented Nov 23, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

sea-bass commented Nov 24, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

AimanHaidar commented Nov 24, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

AimanHaidar commented Dec 1, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

codecov bot commented Dec 1, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

EzraBrooks commented Dec 1, 2025

Uh oh!

AimanHaidar commented Dec 1, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

AimanHaidar commented Dec 2, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

AimanHaidar commented Dec 3, 2025

Uh oh!

AimanHaidar commented Dec 3, 2025

Uh oh!

EzraBrooks commented Dec 3, 2025

Uh oh!

Uh oh!

AimanHaidar commented Dec 4, 2025

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants

AimanHaidar commented Nov 10, 2025 •

edited

Loading

AndyZe left a comment •

edited

Loading

AimanHaidar commented Nov 23, 2025 •

edited

Loading

AimanHaidar commented Nov 23, 2025 •

edited

Loading

sea-bass commented Nov 24, 2025 •

edited

Loading

AimanHaidar commented Nov 24, 2025 •

edited

Loading

AimanHaidar commented Dec 1, 2025 •

edited

Loading

codecov bot commented Dec 1, 2025 •

edited

Loading

AimanHaidar commented Dec 1, 2025 •

edited

Loading

AimanHaidar commented Dec 2, 2025 •

edited

Loading